#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <cmath>

#include "matplotlibcpp.h"

using namespace std;
namespace plt = matplotlibcpp;

typedef struct {
    unsigned long idx;
    double latError;
} minLatError_idx_t;

const char fileName[] = "/home/shukebeita/data/data.txt";

inline double normFunc(double posRefX, double posRefY, double posX, double posY){
    return sqrt(pow(posRefX - posX, 2) + pow(posRefY - posY, 2));
}

/*
double toPi(double angle){
    double res;
    if(angle > M_PI)
        res = angle - 2*M_PI;
    else if(angle < -M_PI)
        res = angle + 2*M_PI;
    else
        res = angle;

    return res;
}
*/

//读取路径点
vector<vector<double>> readPosPoints(vector<double> &XRef, vector<double> &YRef){
    ifstream readFile;
    readFile.open(fileName);

    if(!readFile.is_open()){
        cout << "file failed !!!" << endl;
        exit(0); //表示程序正常退出
    }

    string dataBuff;
    while(getline(readFile, dataBuff)){
        string tempData;
        vector<string> tempStr;
        for(size_t i = 0; i <= dataBuff.length(); i ++){
            if(dataBuff[i] == ',' || dataBuff[i] == '\0'){
                tempStr.push_back(tempData);
                tempData.clear();
            }else{
                tempData += dataBuff[i];
            }
        }

        XRef.push_back(stod(tempStr[0]));
        YRef.push_back(stod(tempStr[1]));
    }

    readFile.close();

    vector<vector<double>> res(2, vector<double>(XRef.size(), 0));
    res[0] = XRef;
    res[1] = YRef;

    // 画出曲线，这里作为验证
    //plt::plot(XRef, YRef, "r-");
    //plt::show();

    return res;
}

//在参考路径点上，寻找距离当前点最近的距离，及所对应的索引
minLatError_idx_t findMinLatError_idx(vector<double> states,
                                      vector<vector<double>> posRef){
    int lenOfPosRef = posRef[0].size();

    vector<double> dist;
    for(int i = 0; i < lenOfPosRef; i++){
        double normRes  = normFunc(posRef[0][i], posRef[1][i], states[0], states[1]);
        dist.push_back(normRes);
    }

    auto minLatErrorPotinter = min_element(dist.begin(), dist.end());
    double minLatError_temp = *minLatErrorPotinter; // 找到当前位置到路径点的最小距离
    int idx = distance(dist.begin(), minLatErrorPotinter); // 找到距离当前位置最近的一个参考轨迹点的序号

    minLatError_idx_t tempMinLatError_idx;
    tempMinLatError_idx.idx = idx;

    if(states[1] < posRef[1][idx])
        tempMinLatError_idx.latError = -minLatError_temp;
    else
        tempMinLatError_idx.latError = minLatError_temp;

    return tempMinLatError_idx;
}

double stanleyControl(unsigned long idxTarget, vector<double> states,
                      double latError, vector<vector<double>> posRef, double k){
    unsigned long lenOfPosRef = posRef[0].size();
    unsigned long numOfAheadPoint = 15;

    vector<double> aheadPoint(2); // 这里用该点近似P2点
    if(idxTarget < lenOfPosRef-numOfAheadPoint){
        aheadPoint = {posRef[0][idxTarget+numOfAheadPoint], posRef[1][idxTarget+numOfAheadPoint]};
    }else{
        aheadPoint = {posRef[0][lenOfPosRef-1], posRef[1][lenOfPosRef-1]};
    }

    double thetaPhi = atan2(aheadPoint[1] - states[1], aheadPoint[0] - states[0]) - states[2];
    //double thetaPhi = toPi(atan((aheadPoint[1] - states[1])/(aheadPoint[0] - states[0])) - states[2]);
    //double thetaY = atan(k*latError/states[3]);
    double thetaY = atan2(k*latError, states[3]);

    double delta = thetaPhi + thetaY;
    return delta;
}

vector<double> updateStates(double a, vector<double> states,
                            double delta, double dt, double L){
    vector<double> tempNewStates(4);
    tempNewStates[0] = states[0] + states[3]*cos(states[2])*dt;
    tempNewStates[1] = states[1] + states[3]*sin(states[2])*dt;
    tempNewStates[2] = states[2] + states[3]*dt*tan(delta)/L;
    tempNewStates[3] = states[3] + a*dt;
    return tempNewStates;
}

int main()
{
    vector<double> XRef;
    vector<double> YRef;
    vector<vector<double>> posRef = readPosPoints(XRef, YRef); //获得的参考路径点（2 X posRef[0].size()）

    // 有关参数
    double k = 0.1; // 增益参数
    double Kp = 0.8; // 速度P控制系数
    double dt = 0.05; // 采样间隔,单位：s
    double L = 2.91; // 车辆轴距，单位：m
    double targetSpeed = 20; // 目标速度，单位：m/s
    vector<double> initStates = {0, 2, 0, 0}; // X, Y, Phi, v
    vector<double> states = initStates;
    vector<vector<double>> statesActual;
    statesActual.push_back(initStates);

    unsigned long idxTarget = 0;

    while(idxTarget < posRef[0].size()-1){
        //在参考路径点上，寻找距离当前点最近的距离，及所对应的索引
        minLatError_idx_t resMinLatError_idx = findMinLatError_idx(states, posRef);

        idxTarget = resMinLatError_idx.idx;

        // 计算前轮转角
        double delta = stanleyControl(idxTarget, states,
                                      resMinLatError_idx.latError, posRef, k);

        // 计算加速度
        double a = Kp*(targetSpeed - states[3])/dt;

        // 更新状态量
        vector<double> newStates = updateStates(a, states, delta, dt, L);
        states = newStates;

        // 保存newStates
        statesActual.push_back(newStates);

    }

    // 保存实际的位置点
    vector<double> XActual;
    vector<double> YActual;
    for(size_t i = 0; i < statesActual.size(); i++){
        for(size_t j = 0; j < statesActual[0].size()-2; j++){
            if(j == 0)
                XActual.push_back(statesActual[i][j]);
            else
                YActual.push_back(statesActual[i][j]);
        }
    }

    // 画图
    //plt::plot(XRef, YRef, "r-", XActual, YActual, "b*");
    //plt::show();

    // 用于动态画出实际的点
    vector<double> plotXActual;
    vector<double> plotYActual;
    for(size_t i = 0; i < XActual.size(); i++) {
        plotXActual.push_back(XActual[i]);
        plotYActual.push_back(YActual[i]);
        if (i % 10 == 0) {
            plt::clf();
            plt::named_plot("refPos", XRef, YRef, "r-");
            plt::named_plot("posActual", plotXActual, plotYActual, "b*");

            // Set x-axis to interval [0,1000000]
            plt::xlim(-10, 210);

            // Add graph title
            plt::title("Sample figure");

            // Enable legend.
            plt::legend();
            // Display plot continuously
            plt::pause(0.1);
        }
    }
    plt::show();

    return 0;
}
